//
// Created by Hoyin on 2023/12/25.
//

#ifndef VISUAL_LOCATION_RELATIVEORIENTATION_H
#define VISUAL_LOCATION_RELATIVEORIENTATION_H

#include "Eigen/Dense"
#include "Coordinate.h"
#include "Structures/PointPair.h"
#include "vector"
#include "opencv2/core.hpp"
#include <iostream>


/**
 * 相对定向
 */
class RelativeOrientation {
public:
	 Vector3d sta0_position;
	 Vector3d sta1_position;
	 Matrix3d sta0_pose;
	 Matrix3d sta1_pose;

	 Vector3d cam0_intrinsic;
	 Vector3d cam1_intrinsic;
};

/**
 * 连续法相对定向
 */
class ContinuousOrient : RelativeOrientation {
private:
	/**
	 * 连续法外方位元素
	 */
	double Phi{};
	double Omega{};
	double Kappa{};
	double Mu{};
	double Nu{};
	double Bx = 1;

	double _cam0_ppm_x;
	double _cam0_ppm_y;
	double _cam1_ppm_x;
	double _cam1_ppm_y;
public:
	typedef struct {
		Eigen::Vector3d RStation;
		Eigen::Matrix3d RotateMatrix;
	} Result;

	/**
	 * 相对定向构造函数
	 * @param cam0_intrinsic mm
	 * @param cam1_intrinsic mm
	 * @param ppm_x 像素密度，pixel/mm
	 * @param ppm_y 像素密度，pixel/mm
	 */
	ContinuousOrient(const Eigen::Vector3d& cam0_intrinsic, const Eigen::Vector3d& cam1_intrinsic, double cam0_ppm[2], double cam1_ppm[2]) {
		this->cam0_intrinsic = cam0_intrinsic;
		this->cam1_intrinsic = cam1_intrinsic;
		this->setParameters(0, 0, 0, 0, 0);
		this->sta0_position = Vector3d(0, 0, 0);
		this->sta0_pose = Matrix3d::Identity();
		_cam0_ppm_x = cam0_ppm[0];
		_cam0_ppm_y = cam0_ppm[1];
		_cam1_ppm_x = cam1_ppm[0];
		_cam1_ppm_y = cam1_ppm[1];
	};

	void setBaselineLength(double bl) {
		this->Bx = bl;
	}

	/**
	 * 设置外方位元素初值
	 * @param phi
	 * @param omega
	 * @param kappa
	 * @param mu
	 * @param nu
	 */
	void setParameters(double phi, double omega, double kappa, double mu, double nu);

	/**
	 * 设置点对
	 * @param lKeyPoints 左片特征点
	 * @param rKeyPoints 右片特征点
	 * @param matches 匹配集合，默认设置 lKeyPoints 对应 query, rKeyPoints 对应 train
	 */
	 void setPointPairs(const std::vector<cv::KeyPoint> &lKeyPoints, const std::vector<cv::KeyPoint> &rKeyPoints, const std::vector<cv::DMatch> &matches);

	/**
	 * 解算相对定向
	 */
	void solve();

	/** 输出
	 *
	 */
	void output() const;

	Result getResult();

private:
	// 同名点对
	std::vector<PointPair<ImageCoord>> vPointPairs;
	// 左片像空辅坐标
	std::vector<ImageAssistCoord> vIacL;
	// 右片像空辅坐标
	std::vector<ImageAssistCoord> vIacR;

	// 判断收敛条件
	static bool isConvergent(const VectorXd& H, double limit) {
		for (int i = 0; i < 5; i++) {
			if (abs(H[i]) > limit) return false;
		}
		return true;
	}
};


#endif //VISUAL_LOCATION_RELATIVEORIENTATION_H
